#include <SPI.h>
#include <LoRa.h>
#include "config.h"
//#include <IBusBM.h>
#include "FlySkyiBusCombined.h"

int counter = 0;
int counter2 = 0;
//IBusBM IBus;    // IBus object
FlySkyIBus IBus;
static uint8_t channel_count = IBUS_MAXCHANNELS;
static uint16_t channel_data [IBUS_MAXCHANNELS];// = {1500, 1500, 1000, 1500, 1020, 1200, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000};
static uint8_t lazy_packets_count = 20;
void set_safe_outputs (uint16_t *channel_data_in); // To set all outputs to safe values
static bool remote_lost = 0;
static bool displayStatus = 0; // 0 for no serial output about controller values, 1 for information
static uint8_t errorType = 0;
static const uint16_t IBUS_LOWER_LIMIT = 1000;  // Set limits for the data sent over iBus
static const uint16_t IBUS_UPPER_LIMIT = 2000;  // Set limits for the data sent over iBus


void setup()
{
  IBus.begin(Serial);    // iBUS object connected to serial0 RX pin
  Serial.begin(115200);

  while (!Serial)
    ;

  Serial.println("LoRa Sender");

  if (!LoRa.begin(915E6))
  {
    Serial.println("Starting LoRa failed!");
    while (1)
      ;
  }
  LoRa.setTxPower(17);
  LoRa.enableCrc();
  LoRa.setSignalBandwidth(500E3);

}

void loop() {
  if (runEvery(20)) {
    remote_lost = 0;
    errorType = 0;

    // Request one frame to be read
    if (IBus.read_loop() == 1) {
      //          if (displayStatus) Serial.println("lost");
      Serial.println("lost");
      remote_lost = 1;
      errorType = 1;
    } else {

    }

    Serial.print("Sending: ");
    for (uint8_t i = 0; i < (counter2 == 0 ? channel_count : 4); i++) {
      channel_data[i] = IBus.readChannel(i);
      //If attempting to send data out of limits then coerce into the limits
      if (channel_data[i] < IBUS_LOWER_LIMIT) {
        channel_data[i] = IBUS_LOWER_LIMIT;
      }
      else if (channel_data[i] > IBUS_UPPER_LIMIT) {
        channel_data[i] = IBUS_UPPER_LIMIT;
      }
      Serial.print(channel_data[i]);
      //      Serial.print(String(IBus.readChannel(i), HEX));
    }
    Serial.print("-");
    Serial.println(counter);
    //    Serial.print("-");
    //    Serial.println(counter2);

    if (remote_lost == 0) {
      // send in async / non-blocking mode
      LoRa.beginPacket();
      for (uint8_t i = 0; i < (counter2 == 0 ? channel_count : 4); i++) {

        LoRa.print(String(channel_data[i], HEX));
      }
      //    LoRa.print("abababababababab ");
      //    LoRa.print(counter);
      LoRa.endPacket(); // true = async / non-blocking mode

      counter++;
      if (counter2 < lazy_packets_count) {
        counter2++;
      } else {
        counter2 = 0;
      }
    }

  }
}

void set_safe_outputs (uint16_t *channel_data_in) {
  channel_data_in[0] = 1500; // Roll control - default = 1500
  channel_data_in[1] = 1500; // Pitch control - default = 1500
  channel_data_in[2] = 1000; // Throttle control - default = 1000
  channel_data_in[3] = 1500; // Rudder control - default = 1500
  channel_data_in[4] = 1000; // Arming control - default = 1000, armed = 1500
  channel_data_in[5] = 1000; // Selectable control - default = 1000, second mode at 2000
  channel_data_in[6] = 1000;
  channel_data_in[7] = 1000;
  channel_data_in[8] = 1000;
  channel_data_in[9] = 1000;
  channel_data_in[10] = 1000;
  channel_data_in[11] = 1000;
  channel_data_in[12] = 1000;
  channel_data_in[13] = 1000;
}

boolean runEvery(unsigned long interval)
{
  static unsigned long previousMillis = 0;
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval)
  {
    previousMillis = currentMillis;
    return true;
  }
  return false;
}
